
import rospy
from sensor_msgs.msg import NavSatFix, Imu
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from std_msgs.msg import Float32
import math


M_PI = math.pi/3

# 输出船的舵角和推力
class propulsion_VRX:
    def __init__(self, propeller_mid, servo_mid):
        self.last_propeller = 0
        self.last_servo = 0
        self.propeller_mid = propeller_mid
        self.servo_mid = servo_mid
        self.propeller_k = (1 - (-1)) / (100.0 - (-100.0))
        self.servo_k = M_PI / (100.0 - (-100.0))

        self.left_angle = rospy.Publisher('/wamv/thrusters/left_thrust_angle', Float32, queue_size=1)
        self.left_cmd = rospy.Publisher('/wamv/thrusters/left_thrust_cmd', Float32, queue_size=1)
        self.right_angle = rospy.Publisher('/wamv/thrusters/right_thrust_angle', Float32, queue_size=1)
        self.right_cmd = rospy.Publisher('/wamv/thrusters/right_thrust_cmd', Float32, queue_size=1)

    def get_trans_propeller(self, propeller):
        propeller_trans = propeller
        # if propeller >= 100:
        #     propeller_trans = 1
        # elif propeller <= -100:
        #     propeller_trans = -1
        # elif propeller == 0:
        #     propeller_trans = self.propeller_mid
        # else:
        #     propeller_trans = self.propeller_k * (propeller + self.propeller_mid)

        if propeller_trans > 1:
            propeller_trans = 1
        elif propeller_trans < -1:
            propeller_trans = -1

        return propeller_trans

    def get_trans_servo(self, servo):
        servo = 100*servo
        if servo >= 100:
            servo_trans = M_PI / 2
        elif servo <= -100:
            servo_trans = -M_PI / 2
        else:
            servo_trans = self.servo_k * (servo + self.servo_mid)

        if servo_trans > M_PI / 2:
            servo_trans = M_PI / 2
        elif servo_trans < -M_PI / 2:
            servo_trans = -M_PI / 2

        return servo_trans

    def manual_act(self, propeller_L, propeller_R, servo):
        power_L = self.get_trans_propeller(propeller_L)
        power_R = self.get_trans_propeller(propeller_R)
        turn = self.get_trans_servo(servo)

        # self.last_propeller = propeller
        # self.last_servo = servo

        f32_value = Float32()
        f32_value_ = Float32()
        f32_value.data = power_L
        f32_value_.data = power_R
        self.left_cmd.publish(f32_value)
        self.right_cmd.publish(f32_value_)

        f32_value.data = turn
        self.left_angle.publish(f32_value)
        self.right_angle.publish(f32_value)

# vrx接收惯导数据
class ROSVRXInfo:
    def __init__(self, update_func_gps, update_func_ea, update_func_ground_speed,
                 update_func_gyro, update_func_acc, update_handle):
        self.m_update_func_gps = update_func_gps
        self.m_update_func_ea = update_func_ea
        self.m_update_func_ground_speed = update_func_ground_speed
        self.m_update_func_gyro = update_func_gyro
        self.m_update_func_acc = update_func_acc
        self.m_update_handle = update_handle
        self.m_quit = False

    def NavSatFix_cb(self, fix):
        if self.m_update_func_gps is not None:
            self.m_update_func_gps(self.m_update_handle, fix.latitude, fix.longitude, fix.altitude)

    def Odometry_cb(self, odom):
        if self.m_update_func_ea is not None:
            quat = (
                odom.pose.pose.orientation.x,
                odom.pose.pose.orientation.y,
                odom.pose.pose.orientation.z,
                odom.pose.pose.orientation.w
            )
            (roll, pitch, yaw) = euler_from_quaternion(quat)
            yaw = (yaw + 2*math.pi) % (2*math.pi) # Ensure yaw is in [0, 2*pi)
            yaw = math.degrees(yaw)
            self.m_update_func_ea(self.m_update_handle, yaw, math.degrees(pitch), math.degrees(roll))

        if self.m_update_func_ground_speed is not None:
            boat_v = odom.twist.twist  # x是船头方向
            ground_speed = math.sqrt(boat_v.linear.x**2 + boat_v.linear.y**2)
            self.m_update_func_ground_speed(self.m_update_handle, ground_speed , boat_v.linear.x , boat_v.linear.y)

        if self.m_update_func_gyro is not None:
            self.m_update_func_gyro(self.m_update_handle,
                                    math.degrees(odom.twist.twist.angular.x),
                                    math.degrees(odom.twist.twist.angular.y),
                                    math.degrees(odom.twist.twist.angular.z))

    def IMU_cb(self, imu):
        if self.m_update_func_acc is not None:
            self.m_update_func_acc(self.m_update_handle, imu.linear_acceleration.x, imu.linear_acceleration.y,
                                   imu.linear_acceleration.z)

       # rospy.loginfo("Latitude: {:.6f}, Longitude: {:.6f}, Altitude: {:.6f}".format(imu.linear_acceleration.z,imu.linear_acceleration.y, imu.linear_acceleration.z))
    def proc_ROSVRX_info(self):

        rospy.loginfo("ROSVRX_info node started")
        # start_time = rospy.Time.now()
        # def get_current_time():
        #     return (rospy.Time.now() - start_time).to_sec()

        rospy.Subscriber('/wamv/robot_localization/gps/filtered', NavSatFix, self.NavSatFix_cb)
        rospy.Subscriber('/wamv/robot_localization/odometry/filtered', Odometry, self.Odometry_cb)
        rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, self.IMU_cb)

        # while not self.m_quit and not rospy.is_shutdown():
        #     current_time = get_current_time()
        #     rospy.loginfo("Received data at {:.6f} seconds".format(current_time))

    def ROSVRX_info_create(self):
        rospy.loginfo("ROSVRX_info create done! handle ptr {}.".format(id(self)))
        self.proc_ROSVRX_info()

